#include <nds.h>
#include <stdio.h>
#include <math.h>
#include "sobel.h"

int main(void)
{
  consoleDemoInit();
  printf("\nFILTRO SOBEL\n");
  
  // Configuración de pantalla principal
  REG_DISPCNT = MODE_5_2D	// Modo5 (permite modo extendido)
    | DISPLAY_BG2_ACTIVE;	// Activar fondo 2
  
  // Configuración de bloque A de la VRAM
  VRAM_A_CR = VRAM_ENABLE	// Habilitar
    | VRAM_A_MAIN_BG;		// Asignar a Main (al comienzo)
  
  // Configuración de fondo 2
  BGCTRL[2] = BG_MAP_BASE(0)	// Al comienzo de la VRAM
    | BgSize_B16_256x256; 	// 256x256, 16bpp
  
  // Matriz de transformación
  bgTransform[2]->xdx = 256;
  bgTransform[2]->ydx = 0;
  bgTransform[2]->xdy = 0;
  bgTransform[2]->ydy = 256;
  bgTransform[2]->dx  = 0;
  bgTransform[2]->dy  = 0;
  
  /* Declaracion de variables*/
  uint8 rgb[3];
  uint8 gris;
  int i;
  uint16 *mapa = sobelBitmap;
  static uint16 pixel[256*256];

  for (i=0;i<256*256;i++){
    rgb[0]= mapa[i] & 31; //rojo
    rgb[1]= (mapa[i] >>5) & 31;//verde
    rgb[2]= mapa[i] >>10;//azul
    gris = (30 *rgb[0] + 59*rgb[1]+11*rgb[2])/100;
    pixel[i]=RGB15(gris,gris,gris) | BIT(15);
  }
  	/*** HASTA AQUÍ BLANCO Y NEGRO ****/
  
//   static uint16  mapaBN[256*256]; //static para forzar que esté en la RAM
  int x,y,j;
  uint p1,p2,p3,p4,p5,p6,p7,p8,p9;
  for (i=1;i<255;i++){
    for(j=1;j<255;j++){
      p1 = pixel[(i-1)*256+j-1];	
      p2 = pixel[(i-1)*256+ j];
      p3 = pixel[(i-1)*256+ j+1];
      p4 = pixel[i*256+ j-1];
      p5 = pixel[i*256+ j];
      p6 = pixel[i*256+ j+1];
      p7 = pixel[(i+1)*256+ j-1];
      p8 = pixel[(i+1)*256+ j];
      p9 = pixel[(i+1)*256+ j+1];
      x = (p1+(p2+p2)+p3-p7-(p8+p8)-p9);
      y = (p3+(p6+p6)+p9-p1-(p4+p4)-p7);
//       mapaBN[256*i+j]= sqrt(x*x+y*y);
      BG_GFX[i*256+j] = sqrt((x*x)+(y*y));

      
    }
  }
//   dmaCopy(mapaBN,BG_GFX,256*256*2);

  
	return 0;
}
